#include "s_debug_msg_handle.h"

#include "h_cc2640r2f_define.h"
#include "h_cc2640r2f_pwm_simple.h"

#include <icall.h>
#include "util.h"
/* This Header file contains all BLE API and icall structure definition */
#include "icall_ble_api.h"

#include "simple_central.h"

typedef enum
{
    S_DBG_MSG_doScan = 0x00,
    S_DBG_MSG_doDiscovery = 0x01,
    S_DBG_MSG_dostopDiscovery = 0x02,
    S_DBG_MSG_KeyHandle = 0x03,
    S_DBG_MSG_UserCMD = 0x04
}S_DBG_MSG_Type;


#define int2hex(x)							((x / 16) * 10 + x % 16)

#define S_MSG_FLASH_DIVIDE_CHAR				0xFF
//16 50 13 # 25 36 -> addr = 165013, len = 2536
static signed char s_dbg_msg_handle_get_2len(unsigned char *data, unsigned short datalen, unsigned short *len1, unsigned short *len2)
{
	//Record the divide character index
	//unsigned char divide_index = 0U;
	
	signed short i = 0U, j = 0U; 
	unsigned short times = 1;
	
	//Init the datas
	*len1 = 0U;
	*len2 = 0U;
	
	for(i = 0U; i < datalen; i++)
	{
		if(data[i] == S_MSG_FLASH_DIVIDE_CHAR)
		{
			break;
		}
	}
	if(i >= datalen)
	{
		//Divide character not found
		return -1;
	}
        
        H_CC2640R2F_UART_Debug("The divi is %d\r\n", i);
	
	//Calculate the len1
	for(j = i - 1; j >= 0; j--)
	{
		*len1 += int2hex(data[j]) * times;
		times *= 100U;
	}
	
	times = 1U;
	//Calculate the len2
	for(j = datalen - 1; j > i; j--)
	{
		*len2 += int2hex(data[j]) * times;
		times *= 100U;
	}
	
	return 0;
	
}


signed char S_Debug_MSG_Handle(unsigned char *data, unsigned short len)
{
//    unsigned short s_dbg_msg_len1 = 0;
//    unsigned short s_dbg_msg_len2 = 0;

//    H_CC2640R2F_UART_Debug("Data len is %d\r\n", len);
    
    switch(data[0])
    {
//        case S_DBG_MSG_doScan:
//
//            H_CC2640R2F_UART_Debug("Set scan phy\r\n");
//            SimpleCentral_doSetScanPhy(0);
//            H_CC2640R2F_UART_Debug("Done\r\n");
//            break;
//
//        case S_DBG_MSG_doDiscovery:
//            H_CC2640R2F_UART_Debug("Do discovery\r\n");
//            SimpleCentral_doDiscoverDevices(1);
//            //SimpleCentral_enqueueMsg_User(0);
//            H_CC2640R2F_UART_Debug("Done\r\n");
//            break;
//
//        case S_DBG_MSG_dostopDiscovery:
//            H_CC2640R2F_UART_Debug("Do stop discovery\r\n");
//            SimpleCentral_doStopDiscovering(1);
//            H_CC2640R2F_UART_Debug("Done\r\n");
//            break;
            
//        case S_DBG_MSG_KeyHandle:
//            SimpleCentral_KeyHandle_API(data[1]);
//            break;

        case S_DBG_MSG_UserCMD:
            //Set the connect Id
            if(S_DBG_MSG_CMD_connect2Device == data[1])
            {
                //Set the user Id
                if(len > 2)
                {
                    SimpleCentral_UserCommand_SetConnectId(int2hex(data[2]));
                }
            }
            SimpleCentral_UserCommand_API(data[1], &(data[2]));
            break;

        default:
            break;
    }
    return 0;
}
